#include "tf_pub.h"
#include <math.h>

using namespace std;


Tfpub::Tfpub():is_connected_(false)
{
    loop_rate_ = 50;
    time_stamp_ = 0;
    flag_init = 0;
    times_ = 0;
    tfb_.reset(new tf2_ros::TransformBroadcaster());
    tf_.reset(new tf2_ros::Buffer());
    tfl_.reset(new tf2_ros::TransformListener(*tf_));

    nh_.param("/tf_pub_node/x", x_, 0.0);
    std::cout<<"x_ is:"<<x_<<std::endl;
    nh_.param("/tf_pub_node/y", y_, 0.0);
    std::cout<<"y_ is:"<<y_<<std::endl;
    nh_.param("/tf_pub_node/theta", theta_, 0.0);
    std::cout<<"theta_ is:"<<theta_<<std::endl;

    odom_pub = nh_.advertise<nav_msgs::Odometry>("odom", 50);
  
}

Tfpub::~Tfpub()
{
   
}


void Tfpub::workCallback(const geometry_msgs::PoseWithCovarianceStamped& p_pose)
{
      boost::mutex::scoped_lock l(mutex_);
    
      flag_init = 1;
      geometry_msgs::Quaternion odom_quat;
      laser_pose_.x = p_pose.pose.pose.position.x;
      laser_pose_.y = p_pose.pose.pose.position.y;
      laser_pose_.theta = tf2::getYaw(p_pose.pose.pose.orientation);
      geometry_msgs::PoseStamped odom_to_map;

      geometry_msgs::PoseWithCovarianceStamped p;
      p.pose.pose.position.x = p_pose.pose.pose.position.x - cos(tf2::getYaw(p_pose.pose.pose.orientation)-theta_)*x_ + sin(tf2::getYaw(p_pose.pose.pose.orientation)-theta_)*y_;
      p.pose.pose.position.y = p_pose.pose.pose.position.y - sin(tf2::getYaw(p_pose.pose.pose.orientation)-theta_)*x_ - cos(tf2::getYaw(p_pose.pose.pose.orientation)-theta_)*y_;


      base_pose_.x = p.pose.pose.position.x;
      base_pose_.y = p.pose.pose.position.y;
      double basepose_theta = tf2::getYaw(p_pose.pose.pose.orientation)-theta_;
      base_pose_.theta = atan2(sin(basepose_theta), cos(basepose_theta)); 
      base_pose_.theta = tf2::getYaw(p_pose.pose.pose.orientation)-theta_;
      pub_basepose.publish(base_pose_);

      nav_msgs::Odometry odom;
      odom.header.stamp = p_pose.header.stamp;
      odom.header.frame_id = "odom";

      //set the position
      odom.pose.pose.position.x = p.pose.pose.position.x;
      odom.pose.pose.position.y = p.pose.pose.position.y;
      odom.pose.pose.position.z = 0.0;
      odom_quat = tf::createQuaternionMsgFromYaw(base_pose_.theta);
      odom.pose.pose.orientation = odom_quat;

      //set the velocity
      odom.child_frame_id = "base_footprint";
      odom.twist.twist.linear.x = 0;
      odom.twist.twist.linear.y = 0;
      odom.twist.twist.angular.z = 0;

      //publish the message
      odom_pub.publish(odom);

      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = p_pose.header.stamp;
      odom_trans.header.frame_id = "odom";
      odom_trans.child_frame_id = "base_footprint";
      odom_trans.transform.translation.x = p.pose.pose.position.x;
      odom_trans.transform.translation.y = p.pose.pose.position.y;
      odom_trans.transform.translation.z = 0.0;
      odom_trans.transform.rotation = odom_quat;

      //send the transform
      odom_broadcaster.sendTransform(odom_trans);

}

void Tfpub::timerCallback(const ros::TimerEvent& e)
{
    //ROS_INFO("Callback  triggered");
    if(flag_init){
      if(last_laser_pose_.x == laser_pose_.x && last_laser_pose_.y == laser_pose_.y && last_laser_pose_.theta == laser_pose_.theta)
      {
        ROS_INFO("laser_pose NO.");
        ros::param::set("/laser_pose",0);
      }else if(last_laser_pose_.x != laser_pose_.x || last_laser_pose_.y != laser_pose_.y || last_laser_pose_.theta != laser_pose_.theta)
      {
        ROS_INFO("laser_pose YES");
        ros::param::set("/laser_pose",1);
      }
    }
    last_laser_pose_ = laser_pose_;
}


bool Tfpub::init()
{
    laser_sub = nh_.subscribe("laserpose", 2, &Tfpub::workCallback, this);
    thread_.start(ecl::generateFunctionObject(&Tfpub::thread_func,*this));
    loop_rate_ = 50;
    timer1 = nh_.createTimer(ros::Duration(1), &Tfpub::timerCallback,this);
    pub_basepose = nh_.advertise<geometry_msgs::Pose2D>("basepose", 50);
    ROS_INFO("Tfpub init!!!");
    return true;
    
}

void Tfpub::thread_func()
{
    ros::Rate rate(loop_rate_);
    while(ros::ok())
    {
        update();
        rate.sleep();
    }
}

void Tfpub::destroy()
{


}

void Tfpub::loop()
{
	ros::spin();
}

bool Tfpub::update()
{

      ros::Duration transform_tolerance_;
      transform_tolerance_.fromSec(0.1);

      if ( true ) //tf_broadcast_ == true)
      {
       
        ros::Time transform_expiration = (ros::Time::now() +
                                          transform_tolerance_);
        geometry_msgs::TransformStamped tmp_tf_stamped;
        tmp_tf_stamped.header.frame_id = "map";//global_frame_id_
        tmp_tf_stamped.header.stamp = transform_expiration;
        tmp_tf_stamped.child_frame_id = "odom"; //odom_frame_id_
        tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform);

        //if(flag_init) this->tfb_->sendTransform(tmp_tf_stamped);

      }
      //ROS_INFO("base to odom transform");
      return true;
}
